#include "NearestNeighbors.h"
#include <algorithm>

using namespace std;

NearestNeighbors::NearestNeighbors(void)
	: tree_(NULL)
{
}


NearestNeighbors::~NearestNeighbors(void)
{
	if (tree_)
		delete tree_;
}

void NearestNeighbors::init(float minX, float maxX, float minY, float maxY)
{
	if (tree_)
		delete tree_;
	tree_ = new QuadTree(QuadTree::AABB(minX, maxX, minY, maxY));
	spaceMin_ = Vector2(minX, minY);
	spaceMax_ = Vector2(maxX, maxY);
	spaceSize_ = spaceMax_ - spaceMin_;
}

void NearestNeighbors::addVertex(const Vector2& v, int index)
{
	tree_->insert(QuadTree::Node(v, index));
	nodes_.push_back(Node(v, index));
}

void NearestNeighbors::kNearestNeighbors(const Vector2& v, std::vector<int>& neighborIndices, unsigned int k)
{
	// exact linear query
	neighborIndices.clear();
	if (nodes_.size() <= k)
	{
		sort(nodes_.begin(), nodes_.end(), NodeDistanceCompare(v));
		for (unsigned int i = 0; i < nodes_.size(); ++i)
			neighborIndices.push_back(nodes_[i].index_);
		return;
	}

	partial_sort(nodes_.begin(), nodes_.begin() + k, nodes_.end(), NodeDistanceCompare(v));

	for (unsigned int i = 0; i < k; ++i)
		neighborIndices.push_back(nodes_[i].index_);
	return;
	
	/*
	// approx. log(n) query
	neighborIndices.clear();
	if (nodes_.size() <= k)
	{
		for (unsigned int i = 0; i < nodes_.size(); ++i)
			neighborIndices.push_back(nodes_[i].index_);
		return;
	}
	float approxRadius = sqrtf(spaceSize_._x * spaceSize_._y * k / nodes_.size());

	while (true)
	{
		Vector2 min(v._x - approxRadius, v._y - approxRadius);
		Vector2 max(v._x + approxRadius, v._y + approxRadius);

		if (min._x < spaceMin_._x)
		{
			float diff = spaceMin_._x - min._x;
			min._x += diff;
			max._x = std::min(max._x + diff, spaceMax_._x);
		}
		if (min._y < spaceMin_._y)
		{
			float diff = spaceMin_._y - min._y;
			min._y += diff;
			max._y += std::min(max._y + diff, spaceMax_._y);
		}

		if (max._x > spaceMax_._x)
		{
			float diff = max._x - spaceMax_._x;
			max._x -= diff;
			min._x = std::max(min._x - diff, spaceMin_._x);
		}
		if (max._y > spaceMax_._y)
		{
			float diff = max._y - spaceMax_._y;
			max._y -= diff;
			min._y = std::max(min._y - diff, spaceMin_._y);
		}

		QuadTree::AABB range(min, max);
		tree_->queryRange(range, neighborIndices);

		if (neighborIndices.size() >= k)
		{
			return;
		}
		else
			approxRadius *= 2.f;
	}
	*/
}